#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：刘骏轩2025讯飞 
@File ：test.py
@Author ：ljx
@Date ：2025/8/1 下午1:59 
"""

from GetCvPicture import PicGetter
import cv2
import rospy
from geometry_msgs.msg import Twist
from LineFind import LineFinder
import threading
import numpy as np
import time

class PD_Controler:
    def __init__(self):
        self.KP = 0.4
        self.KD = 0.5
        self.last_error = 0
        self.last_last_error = 0
        self.max_output = 0.5
        self.target = 0
        self.output = 0

    def Revise(self, now):
        """
        PD 控制角速度
        :param now:当前值，now = 中点坐标的平均值减去320  now > 0 :则说明中线在小车的靠右位置 小车需右转 , now < 0 : 中线在小车的靠左位置 , 小车需左转
        :return: 控制输出
        """
        # 采用增量式PD
        # 角速度为负时,是顺时针，即向右偏 ; 为正时,是逆时针，即向左偏
        error = self.target - now
        self.output += self.KP * (error - self.last_error) + self.KD * (error + self.last_last_error - 2 * self.last_error)

        self.output = np.clip(self.output, -self.max_output, self.max_output)
        self.last_last_error = self.last_error
        self.last_error = error

        return self.output

def get_input():
    global error
    while 1:
        error = float(input("请输入error: "))
        print()
        time.sleep(0.1)

if __name__ == '__main__':
    error = 0
    rospy.init_node('PD_test', anonymous=True)

    input_thread = threading.Thread(target=get_input)
    input_thread.daemon = True  # 设为守护线程（主程序退出时自动结束）
    input_thread.start()

    cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
    PD_control = PD_Controler()
    msg = Twist()
    last_error = 0
    while 1:
        if last_error != error:
            output = PD_control.Revise(error) if abs(error) > 0.05 else 0
            rospy.loginfo(f"output:{output}")
            msg.angular.z = output
            last_error = error
        cmd_pub.publish(msg)
        time.sleep(0.1)
